#!/usr/bin/env python3

import tkinter as tk
from tkinter import ttk, messagebox
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float64
import threading
import math

class YawControlGUI(Node):
    def __init__(self):
        super().__init__('yaw_control_gui')
        
        # ROS2 publisher
        self.target_yaw_publisher = self.create_publisher(Float64, 'target_yaw', 10)
        
        # Initialize GUI
        self.init_gui()
        
        # Current target yaw
        self.current_target_yaw = 0.0
        
        self.get_logger().info("Yaw Control GUI initialized")

    def init_gui(self):
        # Create main window
        self.root = tk.Tk()
        self.root.title("Yaw Angle Controller")
        self.root.geometry("500x400")
        self.root.resizable(True, True)
        
        # Configure grid weights
        self.root.grid_columnconfigure(0, weight=1)
        self.root.grid_rowconfigure(1, weight=1)
        
        # Title frame
        title_frame = ttk.Frame(self.root)
        title_frame.grid(row=0, column=0, sticky="ew", padx=10, pady=5)
        
        title_label = ttk.Label(title_frame, text="机器人Yaw角度控制器", 
                               font=("Arial", 16, "bold"))
        title_label.pack()
        
        # Main control frame
        main_frame = ttk.Frame(self.root)
        main_frame.grid(row=1, column=0, sticky="nsew", padx=10, pady=5)
        main_frame.grid_columnconfigure(1, weight=1)
        
        # Current angle display
        current_frame = ttk.LabelFrame(main_frame, text="当前目标角度", padding=10)
        current_frame.grid(row=0, column=0, columnspan=2, sticky="ew", pady=5)
        
        self.current_angle_var = tk.StringVar(value="0.0°")
        current_angle_label = ttk.Label(current_frame, textvariable=self.current_angle_var,
                                       font=("Arial", 14, "bold"))
        current_angle_label.pack()
        
        # Angle input frame
        input_frame = ttk.LabelFrame(main_frame, text="设置目标角度", padding=10)
        input_frame.grid(row=1, column=0, columnspan=2, sticky="ew", pady=5)
        input_frame.grid_columnconfigure(1, weight=1)
        
        # Degree input
        ttk.Label(input_frame, text="角度 (度):").grid(row=0, column=0, sticky="w", padx=5)
        self.angle_entry = ttk.Entry(input_frame, font=("Arial", 12))
        self.angle_entry.grid(row=0, column=1, sticky="ew", padx=5, pady=2)
        self.angle_entry.insert(0, "0.0")
        
        # Radian input (for convenience)
        ttk.Label(input_frame, text="弧度:").grid(row=1, column=0, sticky="w", padx=5)
        self.radian_entry = ttk.Entry(input_frame, font=("Arial", 12))
        self.radian_entry.grid(row=1, column=1, sticky="ew", padx=5, pady=2)
        self.radian_entry.insert(0, "0.0")
        
        # Bind entry events for automatic conversion
        self.angle_entry.bind('<KeyRelease>', self.on_degree_change)
        self.radian_entry.bind('<KeyRelease>', self.on_radian_change)
        
        # Preset buttons frame
        preset_frame = ttk.LabelFrame(main_frame, text="预设角度", padding=10)
        preset_frame.grid(row=2, column=0, columnspan=2, sticky="ew", pady=5)
        
        # Create preset buttons
        preset_angles = [
            ("0°", 0), ("45°", 45), ("90°", 90), ("135°", 135),
            ("180°", 180), ("-135°", -135), ("-90°", -90), ("-45°", -45)
        ]
        
        for i, (text, angle) in enumerate(preset_angles):
            row = i // 4
            col = i % 4
            btn = ttk.Button(preset_frame, text=text, 
                           command=lambda a=angle: self.set_preset_angle(a))
            btn.grid(row=row, column=col, padx=2, pady=2, sticky="ew")
            preset_frame.grid_columnconfigure(col, weight=1)
        
        # Control buttons frame
        button_frame = ttk.Frame(main_frame)
        button_frame.grid(row=3, column=0, columnspan=2, sticky="ew", pady=10)
        button_frame.grid_columnconfigure(0, weight=1)
        button_frame.grid_columnconfigure(1, weight=1)
        
        # Send button
        self.send_btn = ttk.Button(button_frame, text="发送角度", 
                                  command=self.send_target_yaw,
                                  style="Accent.TButton")
        self.send_btn.grid(row=0, column=0, padx=5, sticky="ew")
        
        # Stop button
        self.stop_btn = ttk.Button(button_frame, text="停止旋转", 
                                  command=self.stop_rotation,
                                  style="Accent.TButton")
        self.stop_btn.grid(row=0, column=1, padx=5, sticky="ew")
        
        # Status frame
        status_frame = ttk.LabelFrame(main_frame, text="状态", padding=10)
        status_frame.grid(row=4, column=0, columnspan=2, sticky="ew", pady=5)
        
        self.status_var = tk.StringVar(value="就绪")
        status_label = ttk.Label(status_frame, textvariable=self.status_var)
        status_label.pack()
        
        # Style configuration
        style = ttk.Style()
        style.configure("Accent.TButton", font=("Arial", 10, "bold"))

    def on_degree_change(self, event):
        try:
            degrees = float(self.angle_entry.get())
            radians = math.radians(degrees)
            self.radian_entry.delete(0, tk.END)
            self.radian_entry.insert(0, f"{radians:.4f}")
        except ValueError:
            pass

    def on_radian_change(self, event):
        try:
            radians = float(self.radian_entry.get())
            degrees = math.degrees(radians)
            self.angle_entry.delete(0, tk.END)
            self.angle_entry.insert(0, f"{degrees:.2f}")
        except ValueError:
            pass

    def set_preset_angle(self, angle_degrees):
        self.angle_entry.delete(0, tk.END)
        self.angle_entry.insert(0, str(angle_degrees))
        self.on_degree_change(None)

    def send_target_yaw(self):
        try:
            target_yaw_rad = float(self.radian_entry.get())
            
            # Normalize angle to [-pi, pi]
            while target_yaw_rad > math.pi:
                target_yaw_rad -= 2 * math.pi
            while target_yaw_rad < -math.pi:
                target_yaw_rad += 2 * math.pi
            
            # Create and publish message
            msg = Float64()
            msg.data = target_yaw_rad
            self.target_yaw_publisher.publish(msg)
            
            # Update current target display
            self.current_target_yaw = target_yaw_rad
            degrees = math.degrees(target_yaw_rad)
            self.current_angle_var.set(f"{degrees:.1f}°")
            
            # Update status
            self.status_var.set(f"已发送目标角度: {degrees:.1f}°")
            
            self.get_logger().info(f"Target yaw sent: {target_yaw_rad:.4f} rad ({degrees:.1f}°)")
            
        except ValueError:
            messagebox.showerror("错误", "请输入有效的数值")
            self.status_var.set("输入错误")

    def stop_rotation(self):
        # Send current yaw as target to stop rotation
        msg = Float64()
        msg.data = self.current_target_yaw
        self.target_yaw_publisher.publish(msg)
        
        self.status_var.set("停止旋转命令已发送")
        self.get_logger().info("Stop rotation command sent")

    def run(self):
        # Start ROS2 spinning in a separate thread
        self.ros_thread = threading.Thread(target=self.ros_spin, daemon=True)
        self.ros_thread.start()
        
        # Run GUI main loop
        self.root.mainloop()

    def ros_spin(self):
        rclpy.spin(self)

def main():
    rclpy.init()
    
    try:
        gui = YawControlGUI()
        gui.run()
    except KeyboardInterrupt:
        pass
    finally:
        rclpy.shutdown()

if __name__ == '__main__':
    main()